
/**
 * when calculating a path using the Djikstra algorithm, One root node has a
 * data entry for every node on the graph. Each of these entries include a cost
 * of traversing from the root node to a specific node, and which by which node
 * that travel cost was achieved.
 */
struct DjikstraData
{
	/** the cost of traversing to this node from the root */
	float cost;
	/** which node you travel across before reaching the node in question */
	GraphNode * previous;
	/** default constructor (default cost to infinity) */
	DjikstraData():	cost(0),previous(0){cost=0;cost=1/cost;}
	DjikstraData(GraphNode * previous, float cost):cost(cost),previous(previous){}
};

#include "templatehashmap.h"

/**
 * @param Q which nodes in the graph still have to be checked
 * @param a_list the DjikstraData map, organized by nodes (from Q)
 * @return the node who's entry has the smallest distance cost
 */
GraphNode * smallestFrom(TemplateVector<GraphNode*> * Q, 
					TemplateHashMap<GraphNode*,DjikstraData> * a_list)
{
	GraphNode * smallNode = Q->get(0);
	float smallDist = a_list->getByKey(smallNode)->cost;
	for(int i = 1; i < Q->size(); ++i)
	{
		GraphNode * n = Q->get(i);
		float distance = a_list->getByKey(n)->cost;
		if(distance < smallDist)
		{
			smallNode = n;
			smallDist = distance;
		}
	}
	return smallNode;
}

/**
 * @param a_graph prints the DjikstraData for these nodes
 * @param a_list the map of DjikstraData to print
 */
void printDjikstraData(TemplateVector<GraphNode> * a_graph, 
			  TemplateHashMap<GraphNode *, DjikstraData> * a_list)
{
	for(int i = 0; i < a_graph->size(); ++i)
	{
		DjikstraData * data = a_list->getByKey(&a_graph->get(i));
		printf("%c<%c %3.1f>  ", a_graph->get(i).value,
			(data->previous)?data->previous->value:'0',
			data->cost);
	}
	printf("\n");
}

/**
 * @param a_graph all of the nodes in a graph
 * @param a_root a node in a_graph to generate path costs from
 * @param out_list where to write the generated path costs to
 * @note: pseudo code from http://en.wikipedia.org/wiki/Djikstra's_algorithm
 */
void dijkstra(TemplateVector<GraphNode> * a_graph, 
			  GraphNode * a_root,
			  TemplateHashMap<GraphNode *, DjikstraData> & out_list)
// 1  function Dijkstra(Graph, root):
{
	float infinity = 0;
	infinity = 1.0/infinity;
// 2      for each vertex v in Graph:           // Initializations
	for(int i = 0; i < a_graph->size(); ++i)
	{
// 3          dist[v] := infinity ;              // Unknown distance function from root to v
// 4          previous[v] := undefined ;         // Previous node in optimal path from root
		GraphNode * v = &a_graph->get(i);
		out_list.set(v, DjikstraData(0,infinity));
// 5      end for ;
	}
// 6      dist[root] := 0 ;                    // Distance from root to root
	out_list.getByKey(a_root)->cost = 0;
// 7      Q := the set of all nodes in Graph ;
	TemplateVector<GraphNode*> Q;
	for(int i = 0; i < a_graph->size(); ++i)
	{
		Q.add(&a_graph->get(i));
	}
//        // All nodes in the graph are unoptimized - thus are in Q
// 8      while Q is not empty:                 // The main loop
	while(Q.size() > 0)
	{
// 9          u := vertex in Q with smallest dist[] ;
		GraphNode * u = smallestFrom(&Q, &out_list);
//10          if dist[u] = infinity:
		if(out_list.getByKey(u)->cost < 0)
		{
//11              break ;                        // all remaining vertices are inaccessible from root
			break;
//12          fi ;
		}
//13          remove u from Q ;
		Q.removeData(u);
//14          for each neighbor v of u:         // where v has not yet been removed from Q.
		for(int i = 0; i < u->edges.size(); ++i)
		{
			GraphEdge * e = &u->edges.get(i);
			GraphNode * v = e->to;
//15              alt := dist[u] + dist_between(u, v) ;
			float alt = out_list.getByKey(u)->cost + e->cost;
//16              if alt < dist[v]:             // Relax (u,v,a)
			DjikstraData * d = out_list.getByKey(v);
			if(alt < d->cost)
			{
//17                  dist[v] := alt ;
				d->cost = alt;
//18                  previous[v] := u ;
				d->previous = u;
//19              fi  ;
			}
//20          end for ;
		}
//21      end while ;
	}
//22      return dist[] ;
//23  end
}

/**
 * before running getPath, a DijkstraData map must be filled by dijkstra()
 * @param a_list a DjikstraData map to generate a path with
 * @param a_target where to find the path to
 * @param out_path where to write the path into
 */
void getPath(TemplateHashMap<GraphNode*,DjikstraData> * a_list,
			 GraphNode * a_target,
			 TemplateVector<GraphNode*> & out_path)
{
//1  S := empty sequence
	out_path.clear();
//2  u := target
	GraphNode * u = a_target;
//3  while previous[u] is defined:
	while(u != 0)
	{
//4      insert u at the beginning of S
		out_path.insert(0, u);
//5      u := previous[u]
		u = a_list->getByKey(u)->previous;
	}
}
